/*
 *  Copyright 2011 Elmer Garduno
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */
 
import lll.wrj4P5.*;
import lll.Loc.*;

Wrj4P5 wii;
  
int DECAY_SPEED = 10;
int SENSORS = 1; // Change here the number of sensors
  
color RED;
color GREEN;
color BLUE;

float BARS_Y;
float PLOTS_Y;
float PATTERNS_Y;

boolean paused = false;

Sensor[] s = new Sensor[SENSORS];

boolean ledsSet = false;
  
void setup()
{
  int w = 250 * SENSORS;
  size(w,600);
  smooth();
  background(1);
  BARS_Y = (height / 4) * .8;
  PLOTS_Y = (height / 4) * 2.2;
  PATTERNS_Y = (height / 4) * 3.5;
  colorMode(RGB, 1, 1, 1, DECAY_SPEED);
  RED = color(1, 0, 0);
  GREEN = color(0, 1, 0);
  BLUE = color(0, 0, 1);
  for (int i = 0; i < s.length ; i++) {
    s[i] = new Sensor(((width / SENSORS) * i) + 25);
  }
  wii = new Wrj4P5(this);
  wii.connect(SENSORS);
} 

void draw() {
  if (wii.isConnecting()) return;
  if (!ledsSet) {
       for (int i = 0; i < s.length ; i++) {
        wii.rimokon(i).setLED(i, true);
        println(i + ":" + wii.rimokon(i).isLED(i));
      }
    ledsSet = true;
  }
  background(1);
  stroke(0);
  fill(0);
  line(0, BARS_Y, width, BARS_Y);
  line(0, PLOTS_Y, width, PLOTS_Y);
  line(0, PATTERNS_Y, width, PATTERNS_Y);
  text("fps: "+ frameRate,width - 100,20);
  for (int i = 0; i < s.length ; i++) { 
    fill(0);
    text("Remote #" + i,s[i].pos,40);
    if (!paused) {
      Loc instance = new Loc(wii.rimokon(i).sensed);
      s[i].update(instance);
    }
    text("Components", s[0].pos,PLOTS_Y - 100);
    text("Acceleration magnitude", s[0].pos, PATTERNS_Y - 75);
    s[i].draw();
  }
}


class Sensor {
   
   LowPassAvg lp = new LowPassAvg(20);
  
   BarDecay bx;
   BarDecay by;
   BarDecay bz;
   
   Plot plot;
   Plot plot2;
   
   PlotSum plotSum;
   
   int pos;
   
   Sensor(int xoff) {
    pos = xoff;
    bx = new BarDecay(xoff + 25, RED, "x");
    by = new BarDecay(bx.pos + 60, BLUE, "y");
    bz = new BarDecay(by.pos + 60, GREEN, "z");
    plot = new Plot(xoff, PLOTS_Y, .8);
    plot2 = new Plot(xoff, PLOTS_Y, 1.2);
    plotSum = new PlotSum(xoff);
   }
   
   void update(Loc loc) {
     plot.update(loc);
     Loc filtered = lp.filter(loc);
     plot2.update(filtered);
     plotSum.update(filtered);
     bx.update(loc.x);
     by.update(loc.y);
     bz.update(loc.z); 
   }
   
   void draw() {  
     plot.draw();
     plot2.draw();
     plotSum.draw();
     bx.draw();
     by.draw();
     bz.draw(); 
   }
}

class Plot {
  LinkedList data = new LinkedList();
  ArrayList buffer = new ArrayList();

  int xOffset;
  float yOffset;
  float wdth;
  
  Plot(int xp, float yp, float w) {
    xOffset = xp;
    yOffset = yp;
    wdth = w;
  }
  
  void update(Loc dt) {
    data.add(dt);
    if (data.size() > 200) {
      data.removeFirst();
    }
  }
  
  void draw() {
    if (data.size() > 1) {
      for (int i= 1; i < data.size(); i++) {
        int len = data.size();
        Loc pt = (Loc) data.get(len - i);      
        pt = pt.mul(-1);
        Loc pr = (Loc) data.get(len - i - 1);      
        pr = pr.mul(-1);
        int xp = (200 - i);
        drawSeg(xp, pr.x, pt.x, RED);  
        drawSeg(xp, pr.y, pt.y, BLUE);  
        drawSeg(xp, pr.z, pt.z, GREEN);  
      } 
    }
  }
  
  void drawSeg(int x, float y1, float y2, color col) {
    stroke(col);  
    strokeWeight(wdth);  
    line(xOffset + (x - 1), yOffset + (y1 * 20), 
         xOffset + x, yOffset + (y2 * 20));  
    strokeWeight(1);  
  }
}


class PlotSum {
  LinkedList data = new LinkedList();

  int xOffset;
  
  PlotSum(int xo) {
    xOffset = xo;
  }
  
  void update(Loc dt) {
    data.add(dt);
    if (data.size() > 200) {
      data.removeFirst();
    }
  }
  
  void draw() {
    if (data.size() > 1) {
      for (int i= 1; i < data.size(); i++) {
        int len = data.size();
        Loc pt = (Loc) data.get(len - i);      
        pt = pt.mul(-1);
        Loc pr = (Loc) data.get(len - i - 1);      
        pr = pr.mul(-1);
        int xp = (200 - i);
        drawSeg(xp, pr.norm() * -1, pt.norm() * -1);  
      } 
    }
  }
  
  void drawSeg(int x, float y1, float y2) {
    stroke(0);  
    strokeWeight(1.2);  
    line(xOffset + (x - 1), PATTERNS_Y + (y1 * 20), 
         xOffset + x, PATTERNS_Y + (y2 * 20));  
    strokeWeight(1);  
  }
}

class BarDecay {
  
  LinkedList lst = new LinkedList();
  int pos;
  color col;
  String label;
  
  BarDecay(int p, color c, String lbl) {
    pos = p;
    col = c;
    label = lbl;
  }
  
  void update(float loc) {
    lst.addFirst(loc);
    if (lst.size() > DECAY_SPEED) {
      lst.removeLast();
    }
  }
  
  void draw() {
    noStroke();
    for (int i=0; i < lst.size(); i++) {
      Float pt = ((Float) lst.get(i)) * -1; 
      fill(col, (DECAY_SPEED - i));
      rect(pos, BARS_Y, 45, pt * 15);  
    } 
    fill(0);
    text(label + ":" + nf((Float) lst.get(0),1,2), pos, BARS_Y + 75);
  } 
}

void buttonPressed(RimokonEvent evt, int rid) {
   paused = !paused;
   if (evt.wasPressed(RimokonEvent.TWO)) println("2");
   if (evt.wasPressed(RimokonEvent.ONE)) println("1");
   if (evt.wasPressed(RimokonEvent.B)) println("B");
   if (evt.wasPressed(RimokonEvent.A)) println("A");
   if (evt.wasPressed(RimokonEvent.MINUS)) println("Minus");
   if (evt.wasPressed(RimokonEvent.HOME)) println("Home");
   if (evt.wasPressed(RimokonEvent.LEFT)) println("Left");
   if (evt.wasPressed(RimokonEvent.RIGHT)) println("Right");
   if (evt.wasPressed(RimokonEvent.DOWN)) println("Down");
   if (evt.wasPressed(RimokonEvent.UP)) println("Up");
   if (evt.wasPressed(RimokonEvent.PLUS)) println("Plus");
}

